import rclpy
from rclpy.node import Node
import numpy as np
import rviz_custom_msgs.msg as rviz_custom_msgs
import random
import time

class VisBotPublisher(Node):
    def __init__(self):
        super().__init__('visbot_publisher')
        self.publisher = self.create_publisher(rviz_custom_msgs.VisbotIDBoxArr, 'visbot_topic', 10)
        self.timer = self.create_timer(1.0, self.publish_message)

    def generate_random_rect(self):
        """生成符合要求的随机矩形参数"""
        rect = rviz_custom_msgs.VisbotRect()
        rect.rect_x = random.uniform(0.1, 0.5)      # 0.1-0.5随机
        rect.rect_y = random.uniform(0.1, 0.5)      # 0.1-0.5随机
        rect.rect_width = random.uniform(0.1, 0.2)  # 0.1-0.2随机
        rect.rect_height = random.uniform(0.1, 0.2) # 0.1-0.2随机
        return rect

    def publish_message(self):
        msg = rviz_custom_msgs.VisbotIDBoxArr()
        msg.timestamp = int(time.time())
        msg.alarm_info = "random_boxes"

        # 第一个检测框 (person)
        box1 = rviz_custom_msgs.VisbotIDBox()
        box1.object_id = 1
        box1.object_name = "person"
        box1.object_conf = 0.95
        box1.rect_color = 0xFF0000  # 红色
        box1.object_rect = self.generate_random_rect()

        # 第二个检测框 (car)
        box2 = rviz_custom_msgs.VisbotIDBox()
        box2.object_id = 2
        box2.object_name = "car"
        box2.object_conf = 0.8
        box2.rect_color = 0x00FF00  # 绿色
        box2.object_rect = self.generate_random_rect()

        msg.object_array = [box1, box2]

        self.publisher.publish(msg)
        print(f"publish visbot message: {msg.timestamp}")

def main():
    rclpy.init()
    node = VisBotPublisher()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
